/*
 * @Description:          
 * @Author: WANG Hao
 * @Date: 2024-04-25 10:08:26
 * @LastEditTime: 2024-04-25 11:52:54
 * @LastEditors: WANG Hao
 */
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#pragma once

#include <opencv2/core.hpp>
#include <Eigen/Dense>
#include <fstream>
#include  <iostream>

/**
 * @brief 
 * 
 * @param name 
 * @param matrix 
 * @return ** template <typename DataType> 
 */
template <typename DataType>
void writeToCSVfile(std::string name, cv::Matx44d cvMat4, const  double & cal_error)
{
    // 转换为Eigen::Matrix4d
    // Eigen::Matrix4d eigen_mat = toMatrix4d(matrix);
    std::ofstream file(name.c_str());
    // file << eigen_mat.format(CSVFormat);
    file << cvMat4.row(0) << "\n"
         << cvMat4.row(1) << "\n"
         << cvMat4.row(2) << "\n"
         << cvMat4.row(3) << "\n"
         << cal_error << "\n";
}

/**
 * @brief 
 * 
 * @param cvMat3 
 * @return ** Eigen::Matrix<double,3,3> 
 */
Eigen::Matrix4d toMatrix4d(const cv::Matx44d & cvMat4)
{
    Eigen::Matrix4d M;

    M << cvMat4(0, 0), cvMat4(0, 1), cvMat4(0, 2), cvMat4(0, 3),
        cvMat4(1, 0), cvMat4(1, 1), cvMat4(1, 2), cvMat4(1, 3),
        cvMat4(2, 0), cvMat4(2, 1), cvMat4(2, 2), cvMat4(2, 3),
        cvMat4(3, 0), cvMat4(3, 1), cvMat4(3, 2), cvMat4(3, 3);
    return M;
}

/**
 * @brief 
 * 
 * @param M 
 * @return cv::Matx44d 
 */
cv::Matx44d invMat(const cv::Matx44d &M)
{
    cv::Matx33d R = M.get_minor<3, 3>(0, 0);
    R = R.t();
    cv::Vec3d t(M(0, 3), M(1, 3), M(2, 3));
    t = -R * t;
    cv::Matx44d out(
        R(0, 0), R(0, 1), R(0, 2), t(0),
        R(1, 0), R(1, 1), R(1, 2), t(1),
        R(2, 0), R(2, 1), R(2, 2), t(2),
        0.0, 0.0, 0.0, 1.0);

    return out;
}

struct Transformation
{
    cv::Matx33d R;
    cv::Vec3d t;

    // Construct an identity transformation.
    Transformation() : R(cv::Matx33d::eye()), t(0., 0., 0.) {}

    // Construct from H
    Transformation(const cv::Matx44d &H) : R(H.get_minor<3, 3>(0, 0)), t(H(0, 3), H(1, 3), H(2, 3)) {}

    // Create homogeneous matrix from this transformation
    cv::Matx44d to_homogeneous() const
    {
        return cv::Matx44d(
            // row 1
            R(0, 0),
            R(0, 1),
            R(0, 2),
            t(0),
            // row 2
            R(1, 0),
            R(1, 1),
            R(1, 2),
            t(1),
            // row 3
            R(2, 0),
            R(2, 1),
            R(2, 2),
            t(2),
            // row 4
            0,
            0,
            0,
            1);
    }

    // Construct a transformation equivalent to this transformation followed by the second transformation
    Transformation compose_with(const Transformation &second_transformation) const
    {
        // get this transform
        cv::Matx44d H_1 = to_homogeneous();
        // get the transform to be composed with this one
        cv::Matx44d H_2 = second_transformation.to_homogeneous();
        // get the combined transform
        cv::Matx44d H_3 = H_1 * H_2;
        return Transformation(H_3);
    }
    Transformation compose_with(const cv::Matx44d &second_transformation) const
    {
        // get this transform
        cv::Matx44d H_1 = to_homogeneous();
        // get the transform to be composed with this one
        cv::Matx44d H_2 = second_transformation;
        // get the combined transform
        cv::Matx44d H_3 = H_1 * H_2;
        return Transformation(H_3);
    }
};
